#include "BoxCollider.h"
#include "SphereCollider.h"

namespace GeorgeLib
{
	BoxCollider::BoxCollider(vec3 size)
		: _size(size)
		, _extents(size / 2)
	{
	}

	vec3 BoxCollider::min(void)
	{
		return wPos - _extents;
	}

	vec3 BoxCollider::max(void)
	{
		return wPos + _extents;
	}

	vec3 BoxCollider::size(void)
	{
		return _size;
	}

	void BoxCollider::size(vec3 v)
	{
		_size(v); _extents(v * .5f);
	}

	vec3 BoxCollider::extents(void)
	{
		return _extents;
	}

	bool BoxCollider::Collides(Collider* collider)
	{
		if (!enabled || this == collider) return false;

		BoxCollider* box = CastCollider(collider);
		SphereCollider* sphere = SphereCollider::CastCollider(collider);

		if (box != NULL &&			// Box-Box Collision
			_size.magnitudeSqr() > 0 && box->_size.magnitudeSqr() > 0)
		{
			return (wPos - box->wPos).magnitudeSqr() <= (_extents + box->_extents).magnitudeSqr();
		}
		else if (sphere != NULL &&	// Box-Sphere Collision
			_size.magnitudeSqr() > 0 && sphere->radius() > 0)
		{
			return sphere->Collides(this);
		}

		return false;
	}

	bool BoxCollider::Contains(Collider* collider)
	{
		if (!enabled || this == collider) return false;

		BoxCollider* box = CastCollider(collider);
		SphereCollider* sphere = SphereCollider::CastCollider(collider);

		if (box != NULL &&			// Box-Box containment
			_size.magnitudeSqr() > 0 && box->_size.magnitudeSqr() > 0)
		{
			return  min().xyzLessThan(box->min()) && box->max().xyzLessThan(max());
		}
		else if (sphere != NULL &&	// Box-Sphere containment
			_size.magnitudeSqr() > 0 && sphere->radius() > 0)
		{
			// boxMin should be less than pos - vec3(radius, radius, radius)
			// boxMax should be more than pos + vec3(radius, radius, radius)
			return min().xyzLessThan(sphere->wPos - vec3(sphere->radius())) &&
				max().xyzLessThan(sphere->wPos - vec3(sphere->radius()));
		}

		return false;
	}

	bool BoxCollider::Contains(vec3 point)
	{
		if (!enabled) return false;

		vec3 boxMin = min(), boxMax = max();

		return min().xyzLessThan(point) && point.xyzLessThan(max());
	}

	void BoxCollider::Contain(Collider* collider)
	{
		if (!enabled || this == collider) return;

		BoxCollider* box = CastCollider(collider);
		SphereCollider* sphere = SphereCollider::CastCollider(collider);

		if (box != NULL &&			// Box-Box containment
			_size.magnitudeSqr() > 0 && box->_size.magnitudeSqr() > 0)
		{
			vec3 amin = min(), amax = max();
			vec3 bmin = box->min(), bmax = box->max(), bpos = box->wPos;

			if (Contains(box))
			{
				for (int i = 0; i < 3; ++i)
				{
					box->wPos[i] = (bmin[i] < amin[i]) ? amin[i] : (bmax[i] > amax[i]) ? amax[i] : bpos[i];
				}
			}
		}
		else if (sphere != NULL &&	// Box-Sphere containment
			_size.magnitudeSqr() > 0 && sphere->radius() > 0)
		{
			vec3 amin = min(), amax = max();
			float brad = sphere->radius();
			vec3 bpos = sphere->wPos, bmin = bpos - brad, bmax = bpos + brad;

			if (Contains(sphere))
			{
				for (int i = 0; i < 3; ++i)
				{
					sphere->wPos[i] = (bmin[i] < amin[i]) ? amin[i] + brad :
						(bmax[i] > amax[i]) ? amax[i] - brad : bpos[i];
				}
			}
		}
	}

	void BoxCollider::Contain(vec3& point)
	{
		if (!enabled) return;

		vec3 boxMin = min(), boxMax = max();

		for (int i = 0; i < 3; ++i)
		{
			point[i] = (point[i] < boxMin[i]) ? boxMin[i] : (point[i] > boxMax[i]) ? boxMax[i] : point[i];
		}
	}

	bool BoxCollider::IsBox(const Collider& collider)
	{
		return dynamic_cast<const BoxCollider*>(&collider) != NULL;
	}

	BoxCollider* BoxCollider::CastCollider(Collider* const collider)
	{
		return dynamic_cast<BoxCollider*>(collider);
	}
}